#include "ActionGo.h"

ActionGo::ActionGo(double maxSpeed, double stopDistance, double controlAngle) :
	ArAction("Go") {
	mySonar = NULL;
	myMaxSpeed = maxSpeed;
	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	setNextArgument(ArArg("maximum speed", &myMaxSpeed, "Maximum speed to go."));
	setNextArgument(ArArg("stop distance", &myStopDistance,
			"Distance at which to stop."));
}

void ActionGo::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (robot == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionGo: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

ArActionDesired *ActionGo::fire(ArActionDesired currentDesired) {
	double range;
	double speed;


	myDesired.reset();

	if (mySonar == NULL) {
		deactivate();
		return NULL;
	}

	range = mySonar->currentReadingPolar(-myControlAngle, myControlAngle)
			- myRobot->getRobotRadius();

	if (range > myStopDistance) {

		speed = range * .3;

		if (speed > myMaxSpeed)
			speed = myMaxSpeed;

		myDesired.setVel(speed);
	}

	else {
		myDesired.setVel(0);
	}
	return &myDesired;
}
